// Copyright (c) 2022 ChenJun
// Licensed under the Apache-2.0 License.

#include <tf2/LinearMath/Quaternion.h>

#include <rclcpp/logging.hpp>
#include <rclcpp/qos.hpp>
#include <rclcpp/utilities.hpp>
#include <serial_driver/serial_driver.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

// C++ system
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include <fstream>
#include <chrono>
#include <ctime>

#include "crc.hpp"
#include "packet.hpp"
#include "rm_serial_driver.hpp"

namespace rm_serial_driver
{
RMSerialDriver::RMSerialDriver(const rclcpp::NodeOptions & options)
: Node("rm_serial_driver", options),
  owned_ctx_{new IoContext(2)},
  serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx_)}
{
  RCLCPP_INFO(get_logger(), "Start RMSerialDriver!");

  getParams();

  // TF broadcaster
  timestamp_offset_ = this->declare_parameter("timestamp_offset", 0.002);
  timestamp_offset_ = this->get_parameter("timestamp_offset").as_double();
  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

  // Create Publisher
  latency_pub_ = this->create_publisher<std_msgs::msg::Float64>("/latency", 10);
  serial_pub_ = this->create_publisher<rm_msgs::msg::ReceiveSerial>("/angle/init", 10);
  status_pub_ = this->create_publisher<rm_msgs::msg::Status>("/status", 10);

  // Detect parameter client
  detector_param_client_ = std::make_shared<rclcpp::AsyncParametersClient>(this, "armor_detector");

  // Tracker reset service client
  reset_tracker_client_ = this->create_client<std_srvs::srv::Trigger>("/tracker/reset");

  try {
    serial_driver_->init_port(device_name_, *device_config_);
    if (!serial_driver_->port()->is_open()) {
      serial_driver_->port()->open();
      receive_thread_ = std::thread(&RMSerialDriver::receiveData, this);
      closecount++;
      //std::cout << "count : " << closecount << std::endl; 
      if(closecount == 10)
      {
        closecount=0;
        exit(0);
      }
    }
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(
      get_logger(), "Error creating serial port: %s - %s", device_name_.c_str(), ex.what());
    throw ex;
    closecount++;
    if(closecount == 10)
      {
        //std::cout << "count : " << closecount << std::endl; 
        closecount=0;
        exit(0);
      }
  }

  aiming_point_.header.frame_id = "odom";
  aiming_point_.ns = "aiming_point";
  aiming_point_.type = visualization_msgs::msg::Marker::SPHERE;
  aiming_point_.action = visualization_msgs::msg::Marker::ADD;
  aiming_point_.scale.x = aiming_point_.scale.y = aiming_point_.scale.z = 0.12;
  aiming_point_.color.r = 1.0;
  aiming_point_.color.g = 1.0;
  aiming_point_.color.b = 1.0;
  aiming_point_.color.a = 1.0;
  aiming_point_.lifetime = rclcpp::Duration::from_seconds(0.1);


  darts_error_sub_ = this->create_subscription<std_msgs::msg::Float64>(
    "/darts/error", 10,
    std::bind(&RMSerialDriver::dartsErrorCallback, this, std::placeholders::_1));

  //----------------------------------------------------------------------------------
    serial_start = std::chrono::steady_clock::now();
    serial_end = std::chrono::steady_clock::now();
    total_count = 0;
    total_time = 0;
}

RMSerialDriver::~RMSerialDriver()
{
  if (receive_thread_.joinable()) {
    receive_thread_.join();
  }

  if (serial_driver_->port()->is_open()) {
    serial_driver_->port()->close();
  }

  if (owned_ctx_) {
    owned_ctx_->waitForExit();
  }
}

void RMSerialDriver::receiveData()
{
  std::vector<uint8_t> header(1);
  std::vector<uint8_t> data;
  data.reserve(sizeof(ReceivePacket));
  bool receiving_data = false;  // 用于跟踪是否正在接收数据
  std::vector<uint8_t> data_buffer;  // 用于存储接收的数据
  uint16_t CRC16_init = 0xFFFF;
  uint16_t CRC_check = 0x0000;
  
  // 添加文件操作相关变量
  std::ofstream data_file;
  
  // 获取当前时间并格式化为字符串
  auto now = std::chrono::system_clock::now();
  std::time_t now_time = std::chrono::system_clock::to_time_t(now);
  std::tm now_tm;
  // 在Ubuntu上使用localtime_r而不是localtime_s
  localtime_r(&now_time, &now_tm);
  char time_str[100];
  std::strftime(time_str, sizeof(time_str), "%Y%m%d%H%M%S", &now_tm);
  
  // 创建文件路径，使用当前时间命名
  std::string file_path = "/home/mechax/zkw/darts_adjust/darts" + std::string(time_str) + ".csv";
  
  // 打开文件（不需要检查文件是否存在，因为每次都是新建文件）
  data_file.open(file_path);
  
  if (!data_file.is_open()) {
    RCLCPP_ERROR(get_logger(), "无法创建数据文件: %s", file_path.c_str());
  } else {
    RCLCPP_INFO(get_logger(), "数据将保存到文件: %s", file_path.c_str());
    // 写入文件头
    data_file << "时间,飞镖编号,是否视觉校准,yaw" << std::endl;
  }

  while (rclcpp::ok()) {
      try {
          // 这一行从串行端口接收一个字节的数据，将其存储在 header 向量中
          serial_driver_->port()->receive(header);
     
          if (receiving_data) {
              // 如果正在接收数据，将数据添加到缓冲区
              data_buffer.push_back(header[0]);
                  // 处理接收到的数据
                  if (data_buffer.size() == sizeof(ReceivePacket) + 1) {
                    receiving_data = false;
                    if (header[0] == 0xAA)
                    {
                      receiving_data = false;
                      ReceivePacket packet = fromVector(data_buffer);

                      CRC_check = crc16::Get_CRC16_Check_Sum(reinterpret_cast<const uint8_t *>(&packet), sizeof(packet)-2,CRC16_init);
                          
                      if(CRC_check == packet.checksum)
                      {
                        // 数据校验成功，保存到文件
                        if (data_file.is_open()) {
                          // 直接使用数据包中的时间（roll字段）
                          // 写入数据，以键值对形式，只保存需要的字段
                          data_file << packet.roll << ","
                                   << static_cast<int>(packet.is_rune) << ","
                                   << static_cast<int>(packet.detect_color) << ","
                                   << packet.yaw << std::endl;
                          
                          RCLCPP_DEBUG(get_logger(), "已保存数据包到文件");
                        }
                      }
                    }
                    // 清空数据缓冲区
                  data_buffer.clear();
                  }
              }
           else if (header[0] == 0x5A) {
              // 如果检测到开始标识符（0x5A），开始接收数据
              receiving_data = true;
              data_buffer.push_back(header[0]);
          }
      } catch (const std::exception & ex) {
          RCLCPP_ERROR_THROTTLE(
              get_logger(), *get_clock(), 20, "Error while receiving data: %s", ex.what());
          reopenPort();
      }
  }
  
  // 关闭文件
  if (data_file.is_open()) {
    data_file.close();
  }
}

void RMSerialDriver::dartsErrorCallback(const std_msgs::msg::Float64 msg)
{
  const static std::map<std::string, uint8_t> id_unit8_map{
    {"", 0},  {"outpost", 0}, {"1", 1}, {"1", 1},     {"2", 2},
    {"3", 3}, {"4", 4},       {"5", 5}, {"guard", 6}, {"base", 7}};

  try {
    SendPacket packet;
    packet.header = 0xA5;
    packet.is_tracking = 0;
    packet.is_can_hit = 0;
    packet.yaw = 0;
    packet.pitch = 0;
    packet.distance = msg.data;
    crc16::Append_CRC16_Check_Sum(reinterpret_cast<uint8_t *>(&packet), sizeof(packet));

    std::vector<uint8_t> data = toVector(packet);
    std::cout <<"--------------------"<< msg.data << std::endl;
    serial_driver_->port()->send(data);
    // std::cout << msg.data << std::endl;
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(get_logger(), "Error while sending data: %s", ex.what());
    reopenPort();
  }
}

void RMSerialDriver::getParams()
{
  using FlowControl = drivers::serial_driver::FlowControl;
  using Parity = drivers::serial_driver::Parity;
  using StopBits = drivers::serial_driver::StopBits;

  uint32_t baud_rate{};
  auto fc = FlowControl::NONE;
  auto pt = Parity::NONE;
  auto sb = StopBits::ONE;

  try {
    device_name_ = declare_parameter<std::string>("device_name", "");
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The device name provided was invalid");
    throw ex;
  }

  try {
    baud_rate = declare_parameter<int>("baud_rate", 0);
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The baud_rate provided was invalid");
    throw ex;
  }

  try {
    const auto fc_string = declare_parameter<std::string>("flow_control", "");

    if (fc_string == "none") {
      fc = FlowControl::NONE;
    } else if (fc_string == "hardware") {
      fc = FlowControl::HARDWARE;
    } else if (fc_string == "software") {
      fc = FlowControl::SOFTWARE;
    } else {
      throw std::invalid_argument{
        "The flow_control parameter must be one of: none, software, or hardware."};
    }
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The flow_control provided was invalid");
    throw ex;
  }

  try {
    const auto pt_string = declare_parameter<std::string>("parity", "");

    if (pt_string == "none") {
      pt = Parity::NONE;
    } else if (pt_string == "odd") {
      pt = Parity::ODD;
    } else if (pt_string == "even") {
      pt = Parity::EVEN;
    } else {
      throw std::invalid_argument{"The parity parameter must be one of: none, odd, or even."};
    }
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The parity provided was invalid");
    throw ex;
  }

  try {
    const auto sb_string = declare_parameter<std::string>("stop_bits", "");

    if (sb_string == "1" || sb_string == "1.0") {
      sb = StopBits::ONE;
    } else if (sb_string == "1.5") {
      sb = StopBits::ONE_POINT_FIVE;
    } else if (sb_string == "2" || sb_string == "2.0") {
      sb = StopBits::TWO;
    } else {
      throw std::invalid_argument{"The stop_bits parameter must be one of: 1, 1.5, or 2."};
    }
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The stop_bits provided was invalid");
    throw ex;
  }

  device_config_ =
    std::make_unique<drivers::serial_driver::SerialPortConfig>(baud_rate, fc, pt, sb);
}

void RMSerialDriver::reopenPort()
{
  RCLCPP_WARN(get_logger(), "Attempting to reopen port");
  try {
    if (serial_driver_->port()->is_open()) {
      serial_driver_->port()->close();
      closecount++;
      //std::cout << "count : " << closecount << std::endl; 
      if(closecount == 10)
      {
        //std::cout << "count : " << closecount << std::endl; 
        closecount=0;
        exit(0);
      }
    }
    serial_driver_->port()->open();
      closecount++;
      //std::cout << "count : " << closecount << std::endl; 
      if(closecount == 10)
      {
        //std::cout << "count : " << closecount << std::endl; 
        closecount=0;
        exit(0);
      }
    RCLCPP_INFO(get_logger(), "Successfully reopened port");
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(get_logger(), "Error while reopening port: %s", ex.what());
    closecount++;
    std::cout << "count : " << closecount << std::endl; 
    if(closecount == 10)
    {
      std::cout << "count : " << closecount << std::endl; 
      closecount=0;
      exit(0);
    }
    if (rclcpp::ok()) {
      rclcpp::sleep_for(std::chrono::seconds(1));
      reopenPort();
    }
  }
}

void RMSerialDriver::setParam(const rclcpp::Parameter & param)
{
  if (!detector_param_client_->service_is_ready()) {
    RCLCPP_WARN(get_logger(), "Service not ready, skipping parameter set");
    return;
  }

  if (
    !set_param_future_.valid() ||
    set_param_future_.wait_for(std::chrono::seconds(0)) == std::future_status::ready) {
    RCLCPP_INFO(get_logger(), "Setting detect_color to %ld...", param.as_int());
    set_param_future_ = detector_param_client_->set_parameters(
      {param}, [this, param](const ResultFuturePtr & results) {
        for (const auto & result : results.get()) {
          if (!result.successful) {
            RCLCPP_ERROR(get_logger(), "Failed to set parameter: %s", result.reason.c_str());
            return;
          }
        }
        RCLCPP_INFO(get_logger(), "Successfully set detect_color to %ld!", param.as_int());
        initial_set_param_ = true;
      });
  }
}

void RMSerialDriver::resetTracker()
{
  if (!reset_tracker_client_->service_is_ready()) {
    RCLCPP_WARN(get_logger(), "Service not ready, skipping tracker reset");
    return;
  }

  auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
  reset_tracker_client_->async_send_request(request);
  RCLCPP_INFO(get_logger(), "Reset tracker!");
}

}  // namespace rm_serial_driver

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rm_serial_driver::RMSerialDriver)
